from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包的共享目录
    package_dir = get_package_share_directory('usb_camera_publisher')
    config_file = os.path.join(package_dir, 'config', 'camera_params.yaml')
    
    # 声明launch参数
    camera_index_arg = DeclareLaunchArgument(
        'camera_index',
        default_value='0',
        description='USB摄像头索引 (通常是0, 1, 2...)'
    )
    
    image_width_arg = DeclareLaunchArgument(
        'image_width',
        default_value='640',
        description='图像宽度'
    )
    
    image_height_arg = DeclareLaunchArgument(
        'image_height',
        default_value='480',
        description='图像高度'
    )
    
    fps_arg = DeclareLaunchArgument(
        'fps',
        default_value='30',
        description='帧率 (FPS)'
    )
    
    topic_name_arg = DeclareLaunchArgument(
        'topic_name',
        default_value='/camera/color/image_raw',
        description='图像发布话题名称'
    )
    
    frame_id_arg = DeclareLaunchArgument(
        'frame_id',
        default_value='camera_frame',
        description='摄像头坐标系ID'
    )
    
    # 创建摄像头节点
    camera_node = Node(
        package='usb_camera_publisher',
        executable='camera_publisher_node.py',
        name='usb_camera_publisher',
        parameters=[{
            'camera_index': LaunchConfiguration('camera_index'),
            'image_width': LaunchConfiguration('image_width'),
            'image_height': LaunchConfiguration('image_height'),
            'fps': LaunchConfiguration('fps'),
            'topic_name': LaunchConfiguration('topic_name'),
            'frame_id': LaunchConfiguration('frame_id'),
        }],
        output='screen'
    )
    
    return LaunchDescription([
        camera_index_arg,
        image_width_arg,
        image_height_arg,
        fps_arg,
        topic_name_arg,
        frame_id_arg,
        camera_node
    ])